/*******************************************************************************
* Copyright (c) 2008, Jeferson Jose de Miranda                                 *
* UDESC - Universidade do Estado de Santa Catarina                             *
* All rights reserved.                                                         *
*                                                                              *
* Redistribution and use in source and binary forms, with or without           *
* modification, are permitted provided that the following conditions are met:  *
*     * Redistributions of source code must retain the above copyright         *
*       notice, this list of conditions and the following disclaimer.          *
*     * Redistributions in binary form must reproduce the above copyright      *
*       notice, this list of conditions and the following disclaimer in the    *
*       documentation and/or other materials provided with the distribution.   *
*     * Neither the name of the Universidade do Estado de Santa Catarina nor   *
*       the names of its contributors may be used to endorse or promote        *
*       products derived from this software without specific prior written     *
*       permission.                                                            *
*                                                                              *
* THIS SOFTWARE IS PROVIDED BY JEFERSON JOSE DE MIRANDA "AS IS" AND ANY        *
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED    *
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE       *
* DISCLAIMED. IN NO EVENT SHALL JEFERSON JOSE DE MIRANDA BE LIABLE FOR ANY     *
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES   *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND  *
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT   *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF     *
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.            *
*******************************************************************************/

/**
 * \file PointSequence.hpp
 *
 * \brief A sequence of 2D points.
 *
 * \author Jeferson Jose de Miranda
 */

#ifndef POINTSEQUENCE_HPP
#define POINTSEQUENCE_HPP

// OpenCV main header.
#include <cv.h>
// Operations between OpenCV's 2D points (CvPoint).
#include "Point2DMath.hpp"
// Exceptions.hpp groups Move-In exception messages.
#include "Exceptions.hpp"

#define LEFT_BOTTOM   10
#define LEFT_UP       11
#define RIGHT_BOTTOM  12
#define RIGHT_UP      13
#define BOTTOM_LEFT   14
#define BOTTOM_RIGHT  15
#define UPPER_LEFT    16
#define UPPER_RIGHT   17

namespace MoveIn
{
    /**
    * \class PointSequence
    *
    * \brief This is an array of 2D points.
    */
    class PointSequence
    {
        private:
        CvSeq *point_sequence;
        CvMemStorage *storage;

        public:
        PointSequence();

        ~PointSequence();

        void append(CvPoint p);

        void insert(CvPoint p,
                    unsigned int index);

        void remove(unsigned int index);

        CvPoint getPoint(int index);

        CvPoint getFirstPoint();

        CvPoint getLastPoint();

        unsigned int size();

        void reset();

        bool equals(CvPoint p,
                    unsigned int index);

        int boundaryPoint(unsigned char boundary);

        int realIndex(int index);

        void reduce(unsigned int distance);

        void smooth(unsigned int distance);

        void range(PointSequence *range,
                   int idx1,
                   int idx2);

        int indexOf(CvPoint pt);

        void resampleByPixelDistance(unsigned int distance);

        void resampleByEuclideanDistance(float distance);

        void clone(PointSequence *ps);

        int positionDistance(CvPoint p1, CvPoint p2);
    };

/*******************************************************************************
* MoveIn::PointSequence::PointSequence                                         *
*******************************************************************************/

    PointSequence::PointSequence()
    {
        storage = NULL;
        storage = cvCreateMemStorage(0);
        if(storage == NULL)
        {
            throw new OutOfMemory();
        }
        else
        {
            point_sequence = cvCreateSeq(CV_32SC2,
                                         sizeof(*point_sequence),
                                         sizeof(CvPoint),
                                         storage);
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::~PointSequence                                        *
*******************************************************************************/

    PointSequence::~PointSequence()
    {
        cvClearSeq(point_sequence);
        cvReleaseMemStorage(&storage);
    }

/*******************************************************************************
* MoveIn::PointSequence::append                                                *
*******************************************************************************/

    void PointSequence::append(CvPoint p)
    {
        cvSeqPush(point_sequence, &p);
    }


/*******************************************************************************
* MoveIn::PointSequence::insert                                                *
*******************************************************************************/

    void PointSequence::insert(CvPoint p, unsigned int index)
    {
        if(index > (unsigned int) point_sequence->total)
        {
            throw new OutOfBounds();
        }
        else
        {
            cvSeqInsert(point_sequence, index, &p);
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::remove                                                *
*******************************************************************************/

    void PointSequence::remove(unsigned int index)
    {
        if(index >= (unsigned int) point_sequence->total)
        {
            throw new OutOfBounds();
        }
        else
        {
            cvSeqRemove(point_sequence, index);
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::getPoint                                              *
*******************************************************************************/

    CvPoint PointSequence::getPoint(int index)
    {
        try
        {
            int idx = realIndex(index);
            CvPoint *p = (CvPoint*) cvGetSeqElem(point_sequence, idx);
            return cvPoint(p->x, p->y);
        }
        catch(std::exception *e)
        {
            throw e;
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::getFirstPoint                                         *
*******************************************************************************/

    CvPoint PointSequence::getFirstPoint()
    {
        return getPoint(0);
    }

/*******************************************************************************
* MoveIn::PointSequence::getLastPoint                                          *
*******************************************************************************/

    CvPoint PointSequence::getLastPoint()
    {
        if(point_sequence->total == 0)
        {
            throw new OutOfBounds();
        }
        else
        {
            return getPoint(point_sequence->total - 1);
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::size                                                  *
*******************************************************************************/

    unsigned int PointSequence::size()
    {
        return point_sequence->total;
    }

/*******************************************************************************
* MoveIn::PointSequence::reset                                                 *
*******************************************************************************/

    void PointSequence::reset()
    {
        cvClearSeq(point_sequence);
    }

/*******************************************************************************
* MoveIn::PointSequence::equals                                                *
*******************************************************************************/

    bool PointSequence::equals(CvPoint p,
                               unsigned int index)
    {
        if(index >= (unsigned int) point_sequence->total)
        {
            throw new OutOfBounds();
        }
        else
        {
            CvPoint pt;
            pt = getPoint(index);
            if(p.x == pt.x && p.y == pt.y)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::boundaryPoint                                         *
*******************************************************************************/

    int PointSequence::boundaryPoint(unsigned char boundary)
    {
        if(size() == 0)
        {
            return -1;
        }
        int index = 0;

        for(unsigned int i=1; i<size(); i++)
        {
            if(boundary == LEFT_BOTTOM)
            {
                if(getPoint(index).x > getPoint(i).x)
                {
                    index = i;
                }
                else if(getPoint(index).x == getPoint(i).x)
                {
                    if(getPoint(index).y < getPoint(i).y)
                    {
                        index = i;
                    }
                }
            }
            if(boundary == LEFT_UP)
            {
                if(getPoint(index).x > getPoint(i).x)
                {
                    index = i;
                }
                else if(getPoint(index).x == getPoint(i).x)
                {
                    if(getPoint(index).y > getPoint(i).y)
                    {
                        index = i;
                    }
                }
            }
            if(boundary == RIGHT_BOTTOM)
            {
                if(getPoint(index).x < getPoint(i).x)
                {
                    index = i;
                }
                else if(getPoint(index).x == getPoint(i).x)
                {
                    if(getPoint(index).y < getPoint(i).y)
                    {
                        index = i;
                    }
                }
            }
            if(boundary == RIGHT_UP)
            {
                if(getPoint(index).x < getPoint(i).x)
                {
                    index = i;
                }
                else if(getPoint(index).x == getPoint(i).x)
                {
                    if(getPoint(index).y > getPoint(i).y)
                    {
                        index = i;
                    }
                }
            }
            if(boundary == BOTTOM_LEFT)
            {
                if(getPoint(index).y < getPoint(i).y)
                {
                    index = i;
                }
                else if(getPoint(index).y == getPoint(i).y)
                {
                    if(getPoint(index).x > getPoint(i).x)
                    {
                        index = i;
                    }
                }
            }
            if(boundary == BOTTOM_RIGHT)
            {
                if(getPoint(index).y < getPoint(i).y)
                {
                    index = i;
                }
                else if(getPoint(index).y == getPoint(i).y)
                {
                    if(getPoint(index).x < getPoint(i).x)
                    {
                        index = i;
                    }
                }
            }
            if(boundary == UPPER_LEFT)
            {
                if(getPoint(index).y > getPoint(i).y)
                {
                    index = i;
                }
                else if(getPoint(index).y == getPoint(i).y)
                {
                    if(getPoint(index).x > getPoint(i).x)
                    {
                        index = i;
                    }
                }
            }
            if(boundary == UPPER_RIGHT)
            {
                if(getPoint(index).y > getPoint(i).y)
                {
                    index = i;
                }
                else if(getPoint(index).y == getPoint(i).y)
                {
                    if(getPoint(index).x < getPoint(i).x)
                    {
                        index = i;
                    }
                }
            }
        }
        return index;
    }

/*******************************************************************************
* MoveIn::PointSequence::realIndex                                             *
*******************************************************************************/

    int PointSequence::realIndex(int index)
    {
        if(size() == 0)
        {
            return -1;
        }
        else if((index % (int) size()) == 0)
        {
            return 0;
        }
        else if(index < 0)
        {
            return size() + (index % (int) size());
        }
        else if(index >= (int) size())
        {
            return index % size();
        }
        else
        {
            return index;
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::reduce                                                *
*******************************************************************************/

    void PointSequence::reduce(unsigned int distance)
    {
        bool reduced = true;
        CvPoint first;
        CvPoint second;
        while(reduced)
        {
            reduced = false;
            if(size() > 1)
            {
                first = getPoint(0);
                for(unsigned int i=1; i<size(); i++)
                {
                    second = getPoint(i);
                    if(Point2DMath::euclideanDistance(first, second)
                       <
                       distance)
                    {
                        PointSequence::remove(i-1);
                        PointSequence::remove(i-1);
                        try
                        {
                            PointSequence::insert(Point2DMath::
                                                  midPoint(first, second),
                                                  i-1);
                        }
                        catch(std::exception *e)
                        {
                            PointSequence::append(Point2DMath::
                                                  midPoint(first, second));
                        }
                        reduced = true;
                        break;
                    }
                    first = second;
                }
            }
        }
        // Compare the first point with the last point.
        if(size() > 1)
        {
            first = getPoint(0);
            second = getPoint(size()-1);
            PointSequence::remove(0);
            PointSequence::remove(size()-1);
            PointSequence::insert(Point2DMath::midPoint(first, second), 0);
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::smooth                                                *
*******************************************************************************/

    void PointSequence::smooth(unsigned int distance)
    {
        PointSequence temp_ps;
        CvPoint pt[distance];
        for(unsigned int i=0; i<size(); i++)
        {
            temp_ps.append(getPoint(i));
        }
        reset();
        for(unsigned int i=0; i<temp_ps.size(); i++)
        {
            pt[0] = temp_ps.getPoint(i);
            for(unsigned int j=1; j<distance; j++)
            {
                pt[j] = temp_ps.getPoint(i+j);
                pt[0].x += pt[j].x;
                pt[0].y += pt[j].y;
            }
            pt[0].x /= distance;
            pt[0].y /= distance;
            append(pt[0]);
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::range                                                 *
*******************************************************************************/

    void PointSequence::range(PointSequence *range,
                              int idx1,
                              int idx2)
    {
        if(size() == 0)
        {
            return;
        }
        idx1 = realIndex(idx1);
        idx2 = realIndex(idx2);
        if(idx1 <= idx2)
        {
            for(int i=idx1; i<idx2; i++)
            {
                range->append(getPoint(i));
            }
        }
        else
        {
            for(unsigned int i=idx2; i<size(); i++)
            {
                range->append(getPoint(i));
            }
            for(int i=0; i<=idx1; i++)
            {
                range->append(getPoint(i));
            }
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::indexOf                                               *
*******************************************************************************/

    int PointSequence::indexOf(CvPoint pt)
    {
        for(unsigned int i=0; i<size(); i++)
        {
            if(getPoint(i).x == pt.x &&
               getPoint(i).y == pt.y)
            {
                return i;
            }
        }
        return -1;
    }

/*******************************************************************************
* MoveIn::PointSequence::resampleByPixelDistance                               *
*******************************************************************************/

    void PointSequence::resampleByPixelDistance(unsigned int distance)
    {
        PointSequence temp_ps;
        for(unsigned int i=0; i<size(); i++)
        {
            temp_ps.append(getPoint(i));
        }
        reset();
        for(unsigned int i=1; i<temp_ps.size()-1; i+=distance)
        {
            append(temp_ps.getPoint(i));
        }
        /**/
        CvPoint first, last;
        first = temp_ps.getPoint(0);
        last = temp_ps.getPoint(temp_ps.size()-1);
        if(Point2DMath::euclideanDistance(first, last) < distance*0,25)
        {
            append(Point2DMath::midPoint(first, last));
            remove(size()-2);
            remove(0);
        }
        /**/
    }

/*******************************************************************************
* MoveIn::PointSequence::resampleByEuclideanDistance                           *
*******************************************************************************/

    void PointSequence::resampleByEuclideanDistance(float distance)
    {
        PointSequence temp_ps;
        for(unsigned int i=0; i<size(); i++)
        {
            temp_ps.append(getPoint(i));
        }
        reset();
        if(temp_ps.size() > 0)
        {
            append(temp_ps.getPoint(0));
        }
        float dist;
        unsigned int idx = 0;
        for(unsigned int i=1; i<temp_ps.size()-1; i+=(unsigned int)distance)
        {
            dist = Point2DMath::euclideanDistance(temp_ps.getPoint(idx),
                                                  temp_ps.getPoint(i));
            if(dist >= ABS(distance))
            {
                append(temp_ps.getPoint(i));
                idx = i;
            }
        }
        /**/
        CvPoint first, last;
        first = temp_ps.getPoint(0);
        last = temp_ps.getPoint(temp_ps.size()-1);
        if(Point2DMath::euclideanDistance(first, last) < distance*0,25)
        {
            append(Point2DMath::midPoint(first, last));
            remove(size()-2);
            remove(0);
        }
        /**/
    }

/*******************************************************************************
* MoveIn::PointSequence::clone                                                 *
*******************************************************************************/

    void PointSequence::clone(PointSequence *ps)
    {
        reset();
        for(unsigned int i=0; i<ps->size(); i++)
        {
            append(ps->getPoint(i));
        }
    }

/*******************************************************************************
* MoveIn::PointSequence::positionDistance                                      *
*******************************************************************************/

    int PointSequence::positionDistance(CvPoint p1, CvPoint p2)
    {
        int idx1, idx2;
        for(unsigned int i=0; i<size(); i++)
        {
            if(p1.x == getPoint(i).x
               &&
               p1.y == getPoint(i).y)
            {
                idx1 = i;
                for(unsigned int j=0; j<size(); j++)
                {
                    if(p2.x == getPoint(i+j).x
                       &&
                       p2.y == getPoint(i+j).y)
                    {
                        idx2 = j;
                        return idx2 - idx1;
                    }
                }
            }
        }
        return -1;
    }
}

#endif


